HEX
CREATE BUFFER 200 ALLOT
20 CONSTANT R_DATA      ( R/W )  
21 CONSTANT R_SECTCNT   ( W/O ) 
22 CONSTANT R_FEATURES  ( W/O ) 
23 CONSTANT R_LBA_LL    ( W/O )
24 CONSTANT R_LBA_LH    ( W/O )
25 CONSTANT R_LBA_HH    ( W/O )
26 CONSTANT R_LBA_HL    ( W/O )
27 CONSTANT R_CMD       ( W/O )
22 CONSTANT R_ERRORS    ( R/O )
27 CONSTANT R_STATUS    ( R/O )
35 CONSTANT R_DEVCTL    ( W/O )
35 CONSTANT R_ALTSTATUS ( R/O )
37 CONSTANT R_DRIVEADDR ( R/O )
1C CONSTANT R_DATLO
1E CONSTANT R_DATHI
4000 CONSTANT LOADERBASE
VARIABLE IOMODE 10 IOMODE !
CREATE COPIER 
00 C, 
21 C, 00 C, 41 C, ( ld hl,4100h )
11 C, 00 C, 01 C, ( ld de,0100h )
01 C, 00 C, 3F C, ( ld bc,3f00h )
ED C, B0 C,       ( ldir )
DB C, 19 C,       ( in a,[19h] ) 
E6 C, 04 C,       ( and 004h )
CA C, 0B C, 40 C, ( jp nz,400bh )
3E C, 31 C,       ( ld a,'1' )
D3 C, 18 C,       ( out [18h],a )
C3 C, 00 C, 01 C, ( jp 0100h )
HERE COPIER - 1- COPIER C!
CREATE SKIPCOPIER
00 C,
21 C, 00 C, 4A C, ( ld hl,4a00h )
11 C, 00 C, DC C, ( ld de,dc00h )
01 C, 00 C, 23 C, ( ld bc,2300h )
ED C, B0 C,       ( ldir )
DB C, 19 C,       ( in a,[19h] ) 
E6 C, 04 C,       ( and 004h )
CA C, 0B C, 40 C, ( jp nz,400bh )
3E C, 33 C,       ( ld a,'3' )
D3 C, 18 C,       ( out [18h],a )
C3 C, 00 C, F2 C, ( jp f200h )
HERE SKIPCOPIER - 1- SKIPCOPIER C!
: HEX16 0 <# # # # # #> TYPE ;
: HEX8 0 <# # # #> TYPE ;
: ADDR HEX16 [CHAR] : EMIT SPACE ;
: DUMPLINE DUP ADDR DUP 10 + SWAP DO I C@ HEX8 SPACE LOOP ;
: DUMPCHAR DUP 20 < OVER 7F > OR IF DROP [CHAR] . THEN EMIT ;
: DUMPASCII DUP 10 + SWAP DO I C@ DUMPCHAR LOOP ;
: DUMP BASE @ >R HEX 0D EMIT DUP 80 + SWAP DO I DUMPLINE 3 SPACES I DUMPASCII CR 10 +LOOP R> BASE ! ;
( Put ports in input mode )
: IDEIN  4F 1D PC! 4F 1F PC! ;
( Put ports in output mode )
: IDEOUT 0F 1D PC! 0F 1F PC! ;
( byte -- Write byte to command register at 0x1f7 )
: CMD IDEOUT R_DATLO PC! 0 R_CMD PC! IDEIN ;
( Generate a read to 0x1f7 )
: READSTATUS R_STATUS PC@ DROP R_DATLO PC@ ;
: WAITREADY BEGIN READSTATUS 80 AND 0= UNTIL ; ( Wait for bit 7 to clear )
: WAITDRQ   BEGIN READSTATUS 08 AND UNTIL ; ( Wait for bit 3 to set )
( Generate a read strobe on 0x3f0, then read the high and low bytes )
: BUFFERREAD16 WAITDRQ 100 0 DO R_DATA PC@ DROP R_DATLO PC@ BUFFER I 2* + C! R_DATHI PC@ BUFFER I 2* + 1+ C! LOOP ;
: BUFFERWRITE16 WAITDRQ IDEOUT 100 0 DO BUFFER I 2* + DUP C@ R_DATLO PC! 1+ C@ R_DATHI PC! 0 R_DATA PC! LOOP IDEIN ;
: BUFFERREAD8 WAITDRQ 200 0 DO R_DATA PC@ DROP R_DATLO PC@ R_DATA PC@ DROP R_DATLO PC@ >< OR BUFFER I + ! 2 +LOOP ;
: WRITEWORD8 DUP R_DATLO PC! 0 R_DATA PC! >< R_DATLO PC! 0 R_DATA PC! ;
: BUFFERWRITE8 WAITDRQ IDEOUT 200 0 DO BUFFER I + @ WRITEWORD8 2 +LOOP IDEIN ;
( Change these to x8 or x16 depending on what we're testing )
: BUFFERREAD IOMODE @ 8 = IF BUFFERREAD8 ELSE BUFFERREAD16 THEN ;
: BUFFERWRITE IOMODE @ 8 = IF BUFFERWRITE8 ELSE BUFFERWRITE16 THEN ;
( byte register -- )
: WREG IDEOUT SWAP FF AND R_DATLO PC! 0 SWAP PC! IDEIN ;
: RREG PC@ DROP R_DATLO PC@ ; 
( Write 04 to the data latch, then strobe the write to 0x3f6 )
: RESET2 IDEOUT 04 R_DATLO PC! 0 R_DEVCTL PC! 00 R_DATLO PC! 0 R_DEVCTL PC! IDEIN ;
: RESET 04 R_DEVCTL WREG 00 R_DEVCTL WREG ;
: IDENT EC CMD BUFFERREAD ;
( sector_count -- )
: SETSECTCNT R_SECTCNT WREG ;
( sector_high sector_low )
: SETLBA 1 SETSECTCNT DUP R_LBA_LL WREG >< R_LBA_LH WREG DUP R_LBA_HL WREG >< 0F AND E0 OR R_LBA_HH WREG ;   
( sector -- )
: SWRITE WAITREADY SETLBA 30 CMD BUFFERWRITE ;
: SREAD WAITREADY SETLBA 20 CMD BUFFERREAD ; 
: STATUS ." S=" R_STATUS RREG . ." E=" R_ERRORS RREG . ;
: BFILL 200 0 DO DUP BUFFER I + C! LOOP DROP ;
: BFILLW 100 0 DO DUP BUFFER I 2* + ! LOOP DROP ;
: BPATTERN 200 0 DO I BUFFER I + C! LOOP ;
: BZERO 0 BFILLW ;
: MEMCMPW 100 0 DO DUP BUFFER I 2* + @ <> IF DROP I UNLOOP EXIT THEN LOOP DROP -1 ;
: TESTIT E5 BFILL 0 1 SWRITE 0 BFILL 0 1 SREAD ;
: GETSECTORCNT IDENT BUFFER 72 + @ >< ;
: DISKZERO GETSECTORCNT BZERO 0 DO 0 I SWRITE I U. LOOP ;
: DISKFILL GETSECTORCNT 0 DO I BFILLW 0 I SWRITE I U. LOOP ;
: DISKTEST GETSECTORCNT 0 DO 0 I SREAD I MEMCMPW DUP -1 <> IF I UNLOOP EXIT THEN DROP I U. LOOP -1 ;
: SETFEATURE WAITREADY R_FEATURES WREG EF CMD WAITREADY STATUS ;
: SETWCACHE 02 SETFEATURE ;
: SET8BIT 01 SETFEATURE ;
: MODE8 RESET WAITREADY SET8BIT 8 IOMODE ! ;
: MODE16 RESET WAITREADY 10 IOMODE ! ;
: READBOOT_ORG 80 0 DO 0 I SREAD BUFFER LOADERBASE 100 + I  80 * +  80 CMOVE LOOP ;
: READBOOT     20 0 DO 0 I SREAD BUFFER LOADERBASE 100 + I 200 * + 200 CMOVE LOOP ;
: WRITECOPIER LOADERBASE 100 0 FILL COPIER 1+ LOADERBASE COPIER C@ CMOVE ; ( Move code to copy 4100..7FFF to 0100..3FFF )
: WRITESKIPCOPIER LOADERBASE 100 0 FILL SKIPCOPIER 1+ LOADERBASE SKIPCOPIER C@ CMOVE ;
: JUMPLOADER C3 5 C! LOADERBASE 6 ! 0 0 BDOS ; ( Shouldn't ever return )
: BOOT R_STATUS 80 AND IF ." BUSY!" ELSE READBOOT WRITECOPIER JUMPLOADER THEN ;
: SKIPBOOT READBOOT WRITESKIPCOPIER JUMPLOADER ;
